#include <opencv2/opencv.hpp>
#include <iostream> 
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <csignal>
#include <pcl/io/pcd_io.h>      
#include <pcl/point_types.h>     
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include "trajectory_formation/backward.hpp"

namespace backward {
backward::SignalHandling sh;
}
using namespace cv;
using namespace std;

Eigen::Matrix3d K;
bool flag = true;
bool camera_flag=false;
ros::Publisher cloud_visual_pub2;
cv::Mat rgb_image;
cv::Mat frame_threshold;
bool getrgbimage=false;


void shutdown_handler(int)
{
    cout<<"shutdown"<<endl;
    flag = false;
} 

void watchSignal() {
    signal(SIGINT, shutdown_handler);
    signal(SIGTERM, shutdown_handler);
    signal(SIGKILL, shutdown_handler);
    signal(SIGQUIT, shutdown_handler);
}

void cameraCallback(sensor_msgs::CameraInfo msg)
{
	// ROS_INFO("ASSSSSSSSSSSSSSS");
	Eigen::Matrix3f tmp;
	tmp<<msg.K[0],msg.K[1],msg.K[2],
		msg.K[3],msg.K[4],msg.K[5],
		msg.K[6],msg.K[7],msg.K[8];
	K=tmp.cast<double>();
	camera_flag=true;


}


void rgbimageCallback(const sensor_msgs::ImageConstPtr& msg)
{
	// ROS_INFO("GET RGB IMAGE");
	cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
	rgb_image=cv_ptr->image;

	cv::Mat hsv = rgb_image.clone();
	frame_threshold = rgb_image.clone();
	cvtColor(rgb_image, hsv, COLOR_BGR2HSV);

	double low_H = 0;
	double low_S = 35;
	double low_V = 46;
	double high_H = 10;
	double high_S = 255;
	double high_V = 255;
	cv::Mat dilated_image;

	cv::inRange(hsv, Scalar(low_H, low_S, low_V), Scalar(high_H, high_S, high_V), dilated_image);
	cv::Mat element = getStructuringElement(MORPH_RECT, Size(5, 8));

	cv::dilate(dilated_image,frame_threshold,element);

	// imshow("out2", frame_threshold);
	// waitKey(30);

	getrgbimage=true;

}

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
	if(!camera_flag)return;
	if(!getrgbimage)return;
	// ROS_INFO("AAAAAAAAA");
	// cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);


	// K<<350.48,0,320.5,
	// 0,350.48,240.5,
	// 0,0,1;
	cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg);

	Mat img=cv_ptr->image;
	double depth_scale=1;
	// double cx=320;
    // double cy=240;
	// double hfov=1.02974;
	// double width=480;
	// double fx=(width/2.0)/tan(hfov/2);
	// double fy=fx;
	// ROS_INFO("fx: %f",fx);


	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
	for (int m = 0; m < img.rows; m++)
	{
		for (int n=0; n < img.cols; n++)
		{
			// int r=rgb_image.at<Vec3b>(m,n)[0] ;
			// int g=rgb_image.at<Vec3b>(m,n)[1] ;
			// int b=rgb_image.at<Vec3b>(m,n)[2] ;
			// if(r>100&&fabs(r-g)>60)
			// {
			// 	ROS_INFO("R G B: %d %d %d m,n: %d %d",r,g,b,m,n);
			// 	continue;
			// }

			int value=frame_threshold.at<uchar>(m,n) ;
			if(value==255)
			{
				continue;
			}

			double d = img.at<float>(m,n);
			// if (d == 0)
			// continue;

			pcl::PointXYZRGB p;
			if(isnan(d))
			{
				// ROS_INFO("NAN");
				p.r=255;
				p.g=0;
				p.b=0;

				d=7;
			}

			else{
				// ROS_INFO("survive R G B: %d %d %d m,n: %d %d",r,g,b,m,n);

				p.r=0;
				p.g=255;
				p.b=0;
			}

			double Zc=depth_scale*d;
			// Eigen::Vector4d cor((n - cx) * Zc / fx,(m - cy) *Zc / fy,Zc,1);
			// Eigen::Vector4d trans_cor=cor;

			// p.x = trans_cor(0);
			// p.y = trans_cor(1);
			// p.z = trans_cor(2);


			Eigen::Vector3d pixel(n,m,1);
			Eigen::Vector3d coord=Zc*K.inverse()*pixel;

			p.x=coord(0);
			p.y=coord(1);
			p.z=coord(2);

			

			cloud1->points.push_back(p);
		}
	}

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>());
    pcl::VoxelGrid<pcl::PointXYZRGB> sor;
    sor.setInputCloud(cloud1);
	double resolution = 0.1;
    sor.setLeafSize(resolution, resolution, resolution);
    sor.filter(*cloud_filtered);


    // convert to ROS message and publish
    sensor_msgs::PointCloud2 cloud_visual;
    pcl::toROSMsg(*cloud_filtered, cloud_visual);

    // message publish should have the same time stamp
    cloud_visual.header.stamp = ros::Time::now();
    cloud_visual.header.frame_id = "map";
    cloud_visual_pub2.publish(cloud_visual);


    // int width=img.cols;
    // int height=img.rows;
    // int channels=img.channels();
	// // ROS_INFO("get depth image");
    // cout<<"get image: "<<"width: "<<width<<" height: "<<height<<" channels: "<<channels<<endl;

}

int main(int argc, char** argv) 
{
    ros::init(argc, argv, "convert_pointcloud");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);

	image_transport::Subscriber depth_sub = it.subscribe("/iris/camera/depth/image_raw", 1, imageCallback);
	image_transport::Subscriber rgb_sub = it.subscribe("/iris/camera/rgb/image_raw", 1, rgbimageCallback);


	cloud_visual_pub2=nh.advertise<sensor_msgs::PointCloud2>("/convert_pointcloud", 1, true);           //map cloud
	ros::Subscriber camera_sub = nh.subscribe("/iris/camera/depth/camera_info", 1, cameraCallback);
    ros::Rate loop_rate(30);

    // Mat frame;
    // int last_time = 0;
    // double t;
    // double fps;
    // int i = 0;
    // char disp_fps[10];
    string filename = "";	
    while (1) 
	{
		ros::spinOnce();
		loop_rate.sleep();
    }
    cv::destroyAllWindows();
    waitKey(0);
    return 0;
}